#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>

// 回调函数：显示RGB图像
void rgbImageCallback(const sensor_msgs::Image::ConstPtr& msg) {
    try {
        // 将ROS图像消息转换为OpenCV格式
        cv::Mat rgb_image = cv_bridge::toCvShare(msg, "bgr8")->image;

        // 显示RGB图像
        cv::imshow("RGB Image", rgb_image);
        cv::waitKey(1);
    } catch (cv_bridge::Exception& e) {
        ROS_ERROR("Could not convert RGB image: %s", e.what());
    }
}

// 回调函数：显示深度图
void depthImageCallback(const sensor_msgs::Image::ConstPtr& msg) {
    try {
        // 将ROS图像消息转换为OpenCV格式
        cv::Mat depth_image = cv_bridge::toCvShare(msg, "16UC1")->image;

        // 将深度图归一化到可视化范围
        cv::Mat depth_image_normalized;
        depth_image.convertTo(depth_image_normalized, CV_8U, 255.0 / 10000.0);

        // 显示深度图
        cv::imshow("Depth Image", depth_image_normalized);
        cv::waitKey(1);
    } catch (cv_bridge::Exception& e) {
        ROS_ERROR("Could not convert Depth image: %s", e.what());
    }
}

int main(int argc, char** argv) {
    // 初始化ROS节点
    ros::init(argc, argv, "image_display_node");

    // 创建节点句柄
    ros::NodeHandle nh;

    // 订阅RGB图像和深度图像话题
    ros::Subscriber rgb_sub = nh.subscribe("/camera/color/image_raw", 10, rgbImageCallback);
    ros::Subscriber depth_sub = nh.subscribe("/camera/depth/image_rect_raw", 10, depthImageCallback);

    // ROS循环
    ros::spin();

    return 0;
}